#include "MappingThread.h"

xQueueHandle MappingThreadQueue;
extern xQueueHandle UARTQueue;
extern xQueueHandle CollisionThreadQueue;
extern xQueueHandle CommandThreadQueue;
extern xQueueHandle DataManagerThreadQueue;
extern xQueueHandle SpeedThreadQueue;
extern xQueueHandle WebQueue;

extern unsigned int checkStart;
Comms_Msg_Struct *Msg;

int MappingModeEnabled;

//Msg_Type_Enum msgTpye;

unsigned char *lcdMsg;
unsigned char buff[4];

unsigned char distance[4];
Comms_Msg_Struct Map[256];
Comms_Msg_Struct lastCommand;
Comms_Msg_Struct * sendCommand;
int mappingIndex;
int curIndex;

void print_hex_packet(uint8_t *packet,uint16_t size,unsigned char *Buffer)
{
    const char *hex_Map = "0123456789ABCDEF";        
	int i,j;
	j=0;
	for (i=0;i<size;i++)
	{
		Buffer[j++] = hex_Map[ packet[i] >>    4 ];
		Buffer[j++] = hex_Map[ packet[i] &  0x0f ];
		//if (i<5) Buffer[j++] = ',';
	}
	Buffer[j++] = 0;
}

void MappingThreadTaskCode( void * pvParameters )
{
	////////////////////////////////////////////////////////////////////////////
	//SIMULATING THE FIRST RUN THROUGH COURSE, WILL EQUAL 0 FOR SUBSEQUENT RUNS
	////////////////////////////////////////////////////////////////////////////
	MappingModeEnabled = 1;

	int i;
	mappingIndex = 0;
	curIndex = 0;
	lastCommand.msgType=MOTORCONTROLLER;
	lastCommand.counter=0;
	lastCommand.size=2;
	for(i = 0; i < 6; i++)
		lastCommand.data[i] = 0;

	for(i = 0; i < 4; i++)
		distance[i] = 0;

	for(;;)
	{
	 	if( MappingThreadQueue != 0 )
    	{	
			if( xQueueReceive( MappingThreadQueue,(void*) &( Msg ), ( portTickType ) 0 ) )
		    {	
				if(Msg->msgType == Web)
				{
					MappingModeEnabled = Msg->data[1];	
					curIndex = 0;
				}
				
				if(MappingModeEnabled)
				{
					if(Msg->msgType == Command && mappingIndex < 256)
					{											
						if(Msg->data[0] != lastCommand.data[0] || Msg->data[1] != lastCommand.data[1])
						{
							Map[mappingIndex].msgType = Mapping;
							lastCommand.data[0] = Map[mappingIndex].data[0] = Msg->data[0];
							lastCommand.data[1] = Map[mappingIndex].data[1] = Msg->data[1];
							Map[mappingIndex].data[2] = distance[0];
							Map[mappingIndex].data[3] = distance[1];
							Map[mappingIndex].data[4] = distance[2];
							Map[mappingIndex].data[5] = distance[3];
							mappingIndex++;
						}
					}
					else if(Msg->msgType == DATAMANAGER)
					{
						distance[0] = Msg->data[0];
						distance[1] = Msg->data[1];
						distance[2] = Msg->data[2];
						distance[3] = Msg->data[3];
					}
				}
			
				else
				{
					if(Msg->msgType == DATAMANAGER)
					{
						if(curIndex < (mappingIndex - 1))
						{
							
							int curDistance = Msg->data[0];
							curDistance = (curDistance<<8) + Msg->data[1];
							curDistance = (curDistance<<8) + Msg->data[2];
							curDistance = (curDistance<<8) + Msg->data[3];
		
							int nextMappedDistance = Map[curIndex + 1].data[2];
							nextMappedDistance = (nextMappedDistance<<8) + Map[curIndex + 1].data[3];
							nextMappedDistance = (nextMappedDistance<<8) + Map[curIndex + 1].data[4];
							nextMappedDistance = (nextMappedDistance<<8) + Map[curIndex + 1].data[5];
	
							if(abs(curDistance - nextMappedDistance) < 20)	 //If next command occurred within 10 cm
							 	curIndex++;						 
	
							if( CommandThreadQueue != 0 )
							{
								sendCommand = &Map[curIndex];
								 
								if(xQueueSend( CommandThreadQueue, (void * ) &sendCommand, ( portTickType ) 0 ) != pdPASS){};
							}
							
						}
					}
				}
			}
		}
	}
}


void Init_MappingThread()
{
	xTaskHandle MappingThreadHandle;	
	/* Create the task, storing the handle.  Note that the passed parameter 
	ucParameterToPass must exist for the lifetime of the task, so in this 
	case is declared static.  If it was just an an automatic stack variable 
	it might no longer exist, or at least have been corrupted, by the time
	the new task attempts to access it. */
	xTaskCreate( MappingThreadTaskCode, ( signed char * ) "Mapping Thread Task", 2*configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY
	, &MappingThreadHandle );
	MappingThreadQueue = xQueueCreate( 10, sizeof(Comms_Msg_Struct *)  );
}